#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/aruco.hpp>
#include <math.h>
#include<time.h>
using namespace std;

void getParameter(cv::Mat &IMX377_intrinsic, cv::Mat &IMX377_distortion, cv::Matx44d &T_IMX2Hand)   //��ȡ����ڲΣ� ����������� ��֤��ȷ
{
	cv::Mat intrinsic = cv::Mat::eye(3, 3, CV_64FC1);
	cv::Mat distortion(5, 1, CV_64FC1);
	intrinsic.at<double>(0, 0) = 3216.83302106920; //fx
	intrinsic.at<double>(0, 2) = 2056.24399543259; //cx
	intrinsic.at<double>(1, 1) = 3218.03993518521; //fy
	intrinsic.at<double>(1, 2) = 1544.09985226399; //cy
	IMX377_intrinsic = intrinsic;
	distortion.at<double>(0, 0) = -0.326633865883428; //k1 ����
	distortion.at<double>(1, 0) = 0.109377372378819;  //k2
	distortion.at<double>(2, 0) = 0;    //p1 ����
	distortion.at<double>(3, 0) = 0;   //p2
	distortion.at<double>(4, 0) = 0;   //p2
	IMX377_distortion = distortion;
	//cv::Matx44d T_IMX2Hand1 = { -0.552007100282377,	0.833838833655675, -0.000980166101122202,	35.2899825123567,
	//	0.712423733207768,	0.472239629629048,	0.519078179632053,	110.403448736110,
	//	0.433290417157071,	0.285836547165619, -0.854726203356541,	233.643541013033,
	//	0,	0,	0,	1, };
	//cv::Matx44d T_IMX2Hand1 = { -0.552007100282377,	0.833838833655675, -0.000980166101122202,	0.0352899825123567,
	//	0.712423733207768,	0.472239629629048,	0.519078179632053,	0.110403448736110,
	//	0.433290417157071,	0.285836547165619, -0.854726203356541,	0.233643541013033,
	//	0,	0,	0,	1, };
	cv::Matx44d T_IMX2Hand1 = { -0.459319611512885,	0.887824906478882, -0.0281501317122277,	7.78875222506758,
		0.584176986084221,	0.325796018557409,	0.743366802609382,	129.385473265516,
		0.669150762839891,	0.324998291884594, -0.668291378713572,	199.317877155609,
		0,	0,	0,	1, };


	T_IMX2Hand = T_IMX2Hand1;
}

cv::Matx44d getRVectorAndTVector2T(cv::Vec3d r, cv::Vec3d t)              //����ת���� �Լ�ƽ���������  ���������� T  ��֤��ȷ
{
	cv::Mat RMatrix;
	cv::Rodrigues(r, RMatrix);
	t = t * 1000;
	//cout << "t" << t << endl;
	cv::Matx44d T = { RMatrix.at<double>(0,0),RMatrix.at<double>(0,1),RMatrix.at<double>(0,2),t[0,0],
		RMatrix.at<double>(1,0),RMatrix.at<double>(1,1),RMatrix.at<double>(1,2),t[0,1],
		RMatrix.at<double>(2,0),RMatrix.at<double>(2,1),RMatrix.at<double>(2,2),t[0,2],
		0,0,0,1 };
	return T;
}
cv::Point2d calculteTheta1(double yx, double yy, double yz)                   //������ͶӰ���� δ��֤
{
	if (yz == 0)
		return cv::Point2d(0, 0);
	double myPI = 3.14159265358979323846;
	double theta1, theta2;
	theta2 = atan2(yx, sqrt(yy*yy + yz*yz));
	if(yy<=0)
		theta1 = acos(yz);
	else
		theta1 = -acos(yz);
	return cv::Point2d(theta1, theta2);
}
cv::Point2d calculteThetaJoint2Joint(double yx, double yy, double yz)                   //������ͶӰ���� δ��֤
{
	if (yy == 0)
		return cv::Point2d(0, 0);
	double myPI = 3.14159265358979323846;
	double theta1, theta2;
	theta2 = atan2(yz, sqrt(yx*yx + yy*yy));
	if (yx <= 0)
		theta1 = acos(yy);
	else
		theta1 = -acos(yy);
	return cv::Point2d(theta1, theta2);
}

void calculateJointT(vector<int> ids, std::vector<cv::Vec3d> rvecs, std::vector<cv::Vec3d> tvecs, cv::Matx44d T_IMX2Hand, vector<cv::Matx44d> &jointT)//��ÿ���ؽ��ϵ�aurco�������ϵ��λ�� ��֤��ȷ
{
	cv::Matx44d T_IMX2P, T_Hand2P;
	for (int i = 0; i < ids.size(); i++)
	{
		T_IMX2P = getRVectorAndTVector2T(rvecs[i], tvecs[i]);
		//cout << "hand2p"<<T_IMX2P << endl;
		T_Hand2P = T_IMX2Hand.inv()*T_IMX2P;
		jointT[ids[i]] = T_Hand2P;
		//cout << ids[i] << " " << T_Hand2P(0, 3) << " " << T_Hand2P(1, 3) << " " << T_Hand2P(2, 3) << endl;
		//cout << ids[i] << "H2p" << T_Hand2P << endl;
		//cout << "hand2p" << T_IMX2Hand.inv() << endl;
		//cout<<calculteTheta(T_Hand2P(0, 1), T_Hand2P(1, 1), T_Hand2P(2, 1))<< endl;
	}
}


void calculateAngle(vector<cv::Matx44d> jointT, vector<cv::Point2d> &theta)       //��ؽڽǣ��Լ����ڹؽڽǵ�ת�� ��֤����
{
	cv::Matx44d T_jointNext2joint;
	for (int i = 0; i < 10; i++)
	{
		if (i == 0)
			theta[i] = calculteTheta1(jointT[i](0, 1), jointT[i](1, 1), jointT[i](2, 1));
		if(i==5)
			theta[i] = calculteTheta1(jointT[i](0, 1), jointT[i](1, 1), jointT[i](2, 1));
		else
		{
			T_jointNext2joint =  jointT[i - 1].inv()*jointT[i];
			cout << T_jointNext2joint << endl;
			theta[i] = calculteThetaJoint2Joint(T_jointNext2joint(0, 1), T_jointNext2joint(1, 1), T_jointNext2joint(2, 1));
		}

	}
}



int main()
{
	cv::Mat image, imageCopy;
	cv::Mat IMX377_intrinsic(3, 3, CV_64FC1), IMX377_distortion(5, 1, CV_64FC1);
	cv::Matx44d T_IMX2Hand;
	getParameter(IMX377_intrinsic, IMX377_distortion, T_IMX2Hand);
	//cout << IMX377_distortion <<endl;
	//cout << IMX377_intrinsic<< endl;
	//cout << T_IMX2Hand << endl;
	image = cv::imread("/media/st/Document/131lab/蛇形臂/code/test053017/test053017/333.jpg");

	//cv::Rect cutImage;
	//cv::Mat ROIForDetect = image(cutImage);
	//clock_t start, end;
	//start = clock();

	int cutStartX = 1000; int cutStartY = 0; int cutWidth = 1500; int cutHeight = 3046;
	cv::Rect cutImage(cutStartX, cutStartY, cutWidth, cutHeight);
	cv::Mat ROIForDetect = image(cutImage);
	//cv::imshow("gg", ROIForDetect);

	image.copyTo(imageCopy);
	cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
	std::vector<int> ids;
	std::vector<std::vector<cv::Point2f>> corners, rejectedCandidates;
	std::vector<cv::Vec3d> rvecs, tvecs;
	cv::aruco::detectMarkers(ROIForDetect, dictionary, corners, ids);
	//end = clock();

	//cout << "��cppԴ�ļ���������ʱ��Ϊ��" << double(end - start) / CLOCKS_PER_SEC;
	for (int i = 0; i < ids.size(); i++)
	{
		for (int j = 0; j < 4; j++)
		{
			corners[i][j].x = corners[i][j].x + cutStartX;
			corners[i][j].y = corners[i][j].y + cutStartY;
		}
		//cout << ids[i] << endl;
		//cout << corners[i] << endl;
	}

	if (ids.size() > 0)
	{
		vector<cv::Point2d> theta(10);
		vector<cv::Matx44d> jointT(10);
		cv::aruco::drawDetectedMarkers(imageCopy, corners, ids);
		cv::aruco::estimatePoseSingleMarkers(corners, 0.04, IMX377_intrinsic, IMX377_distortion, rvecs, tvecs);
		for (int i = 0; i < ids.size(); i++)
			cv::aruco::drawAxis(imageCopy, IMX377_intrinsic, IMX377_distortion, rvecs[i], tvecs[i], 0.02);
		// // calculateJointT(ids, rvecs, tvecs, T_IMX2Hand, jointT);
		// calculateAngle(jointT, theta);

		// for (int i = 0; i < 5; i++)
		// {
		// 	//cout << jointT[i] << endl;
		// 	cout <<i+1 << theta[i]*180/3.1415926 << endl;
		// }
		// for (int i = 5; i < 10; i++)
		// {
		// 	//cout << jointT[i] << endl;
		// 	cout << i-4 << theta[i] * 180 / 3.1415926 << endl;
		// }

	}




	cv::resize(imageCopy, imageCopy, cv::Size2d(ceil(1300), ceil(1012)), 0, 0, cv::INTER_LINEAR);
	cv::imshow("out", imageCopy);


	cv::waitKey(8000);
	return 1;
}




